package tracker;
/****************************************************************************************************************************
 * TrackerThread implements javas concept of a virtual input device which is added to the 
 * scene graph.  This allows Java3d to call the tracker data methods.																							*
 *																															*
 * Written by Ross Smith - Jan 2005																							*
 *																															*
 ****************************************************************************************************************************/

import javax.media.j3d.InputDevice;
import javax.media.j3d.Sensor;
import javax.media.j3d.SensorRead;
import javax.media.j3d.Transform3D;
import javax.vecmath.Quat4d;
import javax.vecmath.Vector3f;


public class TrackerThread implements InputDevice{
	
	private final double rad = Math.PI / 180.0;

	/*Enable/Disable debugging output*/
	private boolean DEBUG = false;

	/*Store all the tracking data internally*/
	private double heading[];
	private double pitch[];
	private double roll[];
	private double X[];
	private double Y[];
	private double Z[];

	private int sensorCount;
	private TrackerSerialPort sport;
	private TrackerPolhemus tracker;
	private SimpleSensor sensors[];
	private SensorRead sensorRead[];
	private Transform3D []newTransform;

	private Transform3D rotTrans = new Transform3D();
	private Vector3f position = new Vector3f();
	private Vector3f basePos = new Vector3f();

	/*Coordinates of the tracker base*/
	private float tbase_x = 0.0f; 
	private float tbase_y = 0.0f;
	private float tbase_z = 0.0f;

	private int fpsCount = 0;
	private int fps = 0;
	private long lastUpdate;

	public TrackerThread (TrackerPolhemus tracker, TrackerSerialPort sport) {

		this.tracker = tracker;
		this.sport = sport;
		sensorCount = tracker.getNumSensors();

		heading = new double[sensorCount];
		pitch = new double[sensorCount];
		roll = new double[sensorCount];
		X = new double[sensorCount];
		Y = new double[sensorCount];
		Z = new double[sensorCount];

		this.sensors = new SimpleSensor[this.sensorCount];
		this.sensorRead = new SensorRead[this.sensorCount];
		this.newTransform = new Transform3D[this.sensorCount];
		for(int i=0;i<this.sensorCount;i++){
			this.sensors[i] = new SimpleSensor(this);
			this.sensorRead[i] = new SensorRead();
			this.newTransform[i] = new Transform3D();
		}

		//FixMe
		this.sensors[0].setPredictionPolicy(Sensor.HEAD_PREDICTOR);
		this.sensors[1].setPredictionPolicy(Sensor.HAND_PREDICTOR);
		this.sensors[0].setPredictor(Sensor.PREDICT_NEXT_FRAME_TIME);

		basePos.set(tbase_x, tbase_y, tbase_z);
		this.lastUpdate = System.currentTimeMillis();
	}

	public Sensor getNonDominantHandTrackerSensor () {    
		if(this.sensorCount>=3)
			return sensors[2];
		else
			return null;
	}

	private long deadTime;						/*Record of the time the FPS dropped to zero*/
	private boolean timingDeadTime = false;		/*Intermediate state used when we are deciding if the tracker is dead*/
	private long noUpdateTimeout = 800;			/*Length of time we use to decide if the tracker is dead in miliseconds*/
	private boolean reAquiringTracker = false;	/*Indicates if we are trying to reaquire the tracker*/
	private int reaquireRetryCount = 0;			/*Count for the number of times we have tried to reaquire the tracker*/

	public void pollAndProcessInput(){
		this.processStreamInput();

		if(fps == 0){
			if(timingDeadTime != true){
				deadTime = System.currentTimeMillis();
				timingDeadTime = true;
				this.reAquiringTracker = true;
			}
			else{
				if(System.currentTimeMillis() > deadTime + noUpdateTimeout){

					/*Do this first so the screen updates*/
					reaquireRetryCount++;				

					/*Get trackers initilisation string*/
					String init = this.tracker.getInitilisationString();

					/*Clean out any old records*/
					this.sport.flush();

					/*Request the firmware number*/
					sport.write("S");

					/*Check for a response for .5s*/
					String response = "";
					long startTime = System.currentTimeMillis();
					long timeOut = 500;

					while(System.currentTimeMillis() < (startTime + timeOut)){
						response = response.concat(sport.readTimed(1, 50));	
					}

					/*See if the tracker is in a ready state and responding correctly*/
					if(response.substring(2,6).compareTo(tracker.getFirmwareIdent()) == 0){
						/*Configure the tracker*/
						for(int length=0;length<init.length();length++){
							sport.write(new Character(init.charAt(length)).toString());
							try{Thread.sleep(1);}catch(Exception e){}
						}
					}		
					/*reset so we dont try again for the timeout period*/
					this.deadTime = System.currentTimeMillis();
				}
			}
		}
		else{
			this.reAquiringTracker = false;
			reaquireRetryCount = 0;
			timingDeadTime = false;
		}    		
	}

	public int getRetryCount(){
		return this.reaquireRetryCount;
	}

	public boolean getReaquiringTracker(){
		return this.reAquiringTracker;
	}

	public int getFPS(){
		return this.fps;
	}

	public void processStreamInput(){

		updateSensors(); 

		for(int i = 0; i < sensorCount; i++) {
			sensorRead[i].setTime (System.currentTimeMillis());

			/* Tracker rotation code converted by Mark Rebane to use Quaternions for 
			 * the removal of Gimbal-Lock effect Calculate the quaternion rotation 
			 * from the euler values */
			double a = pitch[i] * rad,
			b = heading[i] * rad,
			c = -roll[i] * rad;

			Quat4d Qx = new Quat4d(Math.sin(a/2), 0, 0, Math.cos(a/2));		// Pitch
			Quat4d Qy = new Quat4d(0, Math.sin(b/2), 0, Math.cos(b/2));		// Heading
			Quat4d Qz = new Quat4d(0, 0, Math.sin(c/2), Math.cos(c/2));		// Roll
			Quat4d Q  = new Quat4d();
			/* Multiply the x, y, z Quaternions to get a final rotation Quaternion
			 * The order of multiplication is important Qy MUST be first!
			 * If Qx or Qz is first rotation for pitch or roll will be in the same World
			 * coordinate direction regardless of the users heading. */
			Q.mul(Qy, Qx);
			Q.mul(Qz);

			rotTrans.set(Q);

			float scale = .0254f;
			position.set(tbase_x +(float)(Y [i] * scale),(float)(tbase_y + Z [i] * scale),
					tbase_z + (float)(X [i] * scale));

			newTransform[i].set (position);
			newTransform[i].mul (rotTrans);
			sensorRead[i].set (newTransform[i]);

			sensors[i].setNextSensorRead (sensorRead[i]);
			sensors[i].heading = heading[i];
			sensors[i].pitch   = pitch[i];
			sensors[i].roll    = roll[i];
		}

		if(DEBUG)
			for(int i=0;i< this.sensorCount;i++)
				System.out.println("X=" +X[i] + " Y=" + Y[i] + " Z=" + Z[i]);
	}


	public void setNominalPositionAndOrientation() {
		for (int i = 0; i < this.sensorCount; i++) {
			sensorRead[i].setTime (System.currentTimeMillis ());
			position.set (basePos);
			newTransform[i].set (position);
			sensorRead[i].set (newTransform[i]);
			sensors[i].setNextSensorRead (sensorRead[i]);
		}
	}

	public void setProcessingMode(int mode){
	}

	public void start(){
		String init = this.tracker.getInitilisationString();

		/*We need to sleep just for a bit at each CR so we might as well do it for every character*/
		for(int length=0;length<init.length();length++){
			sport.write(new Character(init.charAt(length)).toString());
			try{Thread.sleep(1);}catch(Exception e){}
		}
	}

	public void updateSensors(){
		try{
			/* Read the data string which should have two records one for each sensor */
			String data = sport.read(this.tracker.getMessageLength());

			/*Update the polhemus FPS*/
			if(this.lastUpdate + 500 > System.currentTimeMillis()){
				/*Dont add to the FPS if there is no data but keep calculating it*/
				if(data!=null)
					this.fpsCount += 2;
			} else {
				this.fps = fpsCount;
				this.fpsCount = 0;
				this.lastUpdate = System.currentTimeMillis();
			}

			if(data == null)
				return;

			//String data = "0000000000000000000000000000000000000000000000000000000000000000000000\n0000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000";
			/*Store each sensors data seperatly*/
			String sensorData[] = new String[this.tracker.getNumSensors()];

			/*Find the index of the CR so we know where to split the string*/
			int sensorDataIndex = 0;
			int offset = 0;
			for (int i=0; i < data.length(); i++){
				if ((byte)data.charAt(i)==13){ /*When we find a CR*/
					sensorData[sensorDataIndex++] = data.substring(offset, i).trim();
					offset = i+1; /*Cut off the CR from the string*/
				}
			}

			if(DEBUG)
				for (int i = 0; i < this.tracker.getNumSensors(); i++)
					System.out.println("Sensor string = " + i + " = "+ sensorData[i]);

			/* Now parse all the sensors data */
			for(int sensor=0;sensor < this.tracker.getNumSensors();sensor++){

				/* Use the sensor number supplied by the tracker this is a random starting index */
				int sensorNumber = Integer.parseInt(sensorData[sensor].substring(0, 2));	

				/*Base offset which varies depenging on the tracker*/
				int o = this.tracker.getPositionOffset();

				double heading = Double.parseDouble(sensorData[sensor].substring(o, o + 7));
				double pitch = Double.parseDouble(sensorData[sensor].substring(o + 7, o + 14));
				double roll = Double.parseDouble(sensorData[sensor].substring(o + 14, o + 21));

				double x = Double.parseDouble(sensorData[sensor].substring(o+21, o+28));
				double y = Double.parseDouble(sensorData[sensor].substring(o+28, o+35));
				double z = Double.parseDouble(sensorData[sensor].substring(o+35, o+42));

				this.heading[sensorNumber - 1] = heading;
				this.pitch[sensorNumber - 1] = pitch;

				/* when we are setting the head allow for the orientation of the sensor */
				if ((sensorNumber - 1) == 0){
					this.roll[sensorNumber - 1] = - roll + 180;
				} else {
					/* Not sure why this needs to be done, I think it might be because 
					 * of the hemisphere setup for each of the trackers is different */
					this.roll[sensorNumber - 1] = -roll;
				}

				double scale = 1;

				if(this.tracker.getTrackerType() == "IsotrakII"){
					scale = 2;
					this.X[sensorNumber - 1] = x * scale;
					this.Y[sensorNumber - 1] = y * scale;
					this.Z[sensorNumber - 1] = z * scale;
				}
				else if(this.tracker.getTrackerType() == "Fastrak"){
					scale = 1;
					this.X[sensorNumber - 1] = x * scale;
					this.Y[sensorNumber - 1] = y * scale;
					this.Z[sensorNumber - 1] = z * scale;
				}
			}

			if(DEBUG)
				for(int allSensors=0; allSensors< this.tracker.getNumSensors();allSensors++){

					System.out.print((allSensors+1) + " - [Heading = " + this.heading[allSensors]);
					System.out.print("] [Pitch = " + this.pitch[allSensors]);
					System.out.print("] [Roll = " + this.roll[allSensors] + "]");

					System.out.print("[X = " + this.X[allSensors]);
					System.out.print("] [Y = " + this.Y[allSensors]);
					System.out.println("] [Z = " + this.Z[allSensors] + "]\n");
				}

		}
		catch(Exception e){
			/*Only print output debuggubg data while debugging, it will make FPS slow*/
			if(DEBUG)
				e.printStackTrace();
		}

	}

	public Sensor getSensor(int channel){
		return sensors[channel];
	}

	public void close(){

	}

	public int getProcessingMode(){ return InputDevice.NON_BLOCKING; }

	public int getSensorCount(){ return this.sensorCount; }

	public boolean initialize() {   	
		return true;
	}

	public Sensor getDominantHandTrackerSensor () { return sensors[1]; }

	public TrackerPolhemus getTracker(){ return this.tracker; }

	
	
	public class SimpleSensor extends Sensor {

		protected double roll    = 0.0;
		protected double pitch   = 0.0;
		protected double heading = 0.0;
			
		public SimpleSensor(InputDevice id){ super(id); }
		public double getRoll(){return roll;}
		public double getPitch(){return pitch;}
		public double getHeading(){return heading;}
	}


}
